#include <iostream>
#include <cmath>
#include "../include/cal_obs/GPS2ENU.h"


const double a = 6378137.0;  // WGS84椭球体的半长轴
const double f = 1.0 / 298.257223563;  // WGS84椭球体的扁率
const double e2 = f * (2 - f);  // 第一偏心率的平方

std::vector<double> wgs84_to_ecef(double lon, double lat, double alt) {
    // 计算第一辅助参数
    double N = a / std::sqrt(1 - e2 * std::sin(lat) * std::sin(lat));

    // 计算ECEF坐标
    double x = (N + alt) * std::cos(lat) * std::cos(lon);
    double y = (N + alt) * std::cos(lat) * std::sin(lon);
    double z = (N * (1 - e2) + alt) * std::sin(lat);

    return {x, y, z};
}

std::vector<double> ecef_to_wgs84(double x, double y, double z) {//这里问题
    // 计算经度
    double lon = std::atan2(y, x);
    double p = std::sqrt(x * x + y * y);
    
    // 初始纬度计算
    double lat = std::atan2(z, p * (1 - e2));
    double N;

    // 迭代计算纬度，直到收敛
    for (int i = 0; i < 10; ++i) {
        N = a / std::sqrt(1 - e2 * std::sin(lat) * std::sin(lat));
        lat = std::atan2(z + e2 * N * std::sin(lat), p);
    }

    // 计算高度
    N = a / std::sqrt(1 - e2 * std::sin(lat) * std::sin(lat));
    double alt = p / std::cos(lat) - N;

    return {lon, lat, alt}; // 转换为度
}

std::vector<double> enu_to_wgs84(const std::vector<double>& enu, double ref_lon, double ref_lat, double ref_alt) {
    // 参考点的ECEF坐标
    auto ref_ecef = wgs84_to_ecef(ref_lon, ref_lat, ref_alt);
    double ref_x = ref_ecef[0];
    double ref_y = ref_ecef[1];
    double ref_z = ref_ecef[2];

    // ENU坐标
    double east = enu[0];
    double north = enu[1];
    double up = enu[2];

    // 将ENU坐标转换为ECEF坐标
    double sin_lon = std::sin(ref_lon);
    double cos_lon = std::cos(ref_lon);
    double sin_lat = std::sin(ref_lat);
    double cos_lat = std::cos(ref_lat);

    double x = ref_x - east * sin_lon - north * cos_lon * sin_lat + up * cos_lat;
    double y = ref_y + east * cos_lon - north * sin_lon * sin_lat + up * sin_lat;
    double z = ref_z + north * cos_lat + up * sin_lat;
    std::cout << "ecef: 东向: " << x << ", 北向: " << y << ", 高向: " << z << std::endl;
    // 将ECEF坐标转换为经纬度和高度
    return ecef_to_wgs84(x, y, z);
}

std::vector<double> wgs84_to_enu(const std::vector<double>& wgs84, double ref_lon, double ref_lat, double ref_alt) {
    // 将WGS84坐标转换为ECEF坐标
    auto ecef = wgs84_to_ecef(wgs84[0], wgs84[1], wgs84[2]);
    double x = ecef[0];
    double y = ecef[1];
    double z = ecef[2];
    std::cout << "ecef: 东向: " << x << ", 北向: " << y << ", 高向: " << z << std::endl;
    // 参考点的ECEF坐标
    auto ref_ecef = wgs84_to_ecef(ref_lon, ref_lat, ref_alt);
    double ref_x = ref_ecef[0];
    double ref_y = ref_ecef[1];
    double ref_z = ref_ecef[2];

    // 计算目标点相对于参考点的坐标差
    double dx = x - ref_x;
    double dy = y - ref_y;
    double dz = z - ref_z;

    // 计算参考点的ENU坐标系转换矩阵
    double sin_lon = std::sin(ref_lon);
    double cos_lon = std::cos(ref_lon);
    double sin_lat = std::sin(ref_lat);
    double cos_lat = std::cos(ref_lat);

    // 将ECEF坐标转换为ENU坐标
    double east = -sin_lon * dx + cos_lon * dy;
    double north = -sin_lat * cos_lon * dx - sin_lat * sin_lon * dy + cos_lat * dz;
    double up = cos_lat * cos_lon * dx + cos_lat * sin_lon * dy + sin_lat * dz;

    return {east, north, up};
}

//int main() {
//    // 示例调用
//    std::vector<double> wgs84 = {119.62583370423 * m_pi / 180, 37.185856513219 * m_pi / 180, 50.0}; // 经度, 纬度, 高度
//    double ref_lon = 119.625333783036 * m_pi / 180;  // 参考点的经度
//    double ref_lat = 37.185030906281 * m_pi / 180;   // 参考点的纬度
//    double ref_alt = 50.0;                    // 参考点的高度
//
//    // wgs84 转 enu
//    auto enu_result = wgs84_to_enu(wgs84, ref_lon, ref_lat, ref_alt);
//    std::cout << "enu坐标: 东向: " << enu_result[0] << ", 北向: " << enu_result[1] << ", 高向: " << enu_result[2] << std::endl;
//
//    // enu坐标示例
//    std::vector<double> enu = {enu_result[0], enu_result[1], 0.0};  // 使用之前转换的enu坐标
//
//    // enu 转 wgs84
//    auto wgs84_result = enu_to_wgs84(enu, ref_lon, ref_lat, ref_alt);
//    std::cout << "经度: " << wgs84_result[0] * 180 / m_pi << ", "
//              << "纬度: " << wgs84_result[1] * 180 / m_pi << ", "
//              << "高度: " << wgs84_result[2] << std::endl;
//
//    // 更新参考点为转换后的结果
//    wgs84={wgs84_result[0],wgs84_result[1],wgs84_result[2]};
//
//    // wgs84 转 enu
//    enu_result = wgs84_to_enu(wgs84, ref_lon, ref_lat, ref_alt);
//    std::cout << "enu坐标: 东向: " << enu_result[0] << ", 北向: " << enu_result[1] << ", 高向: " << enu_result[2] << std::endl;
//
//    return 0;
//}

